package org.noote.libs.robot;

import org.noote.libs.skeleton.Bone;
import org.noote.libs.sogle.Vector3D;

public class Robot_ServoMotor {
	
	protected int _iServo = 0;
	protected boolean _bInversed = false;
	protected float _fPositionMin = 0, _fPositionCenter = 90, _fPositionMax=180;
	protected float _fVirtualPosition = 60, _fPosition = 0, _fOldPosition = 0, _fLastTransmitPosition=-1;
	
	protected float _fSpeedByDegree = 0.2f/60.0f;
	
	protected Bone _bone;
	protected Vector3D _Axis = new Vector3D(0, 0, 1);
	protected float _fAxisOffset = 0;
	
	protected float _axisSize = 1.25f;
	
	public Robot_ServoMotor(int iServo)
	{
		_iServo = iServo;
	}
	
	public Robot_ServoMotor(int iServo, float iMin, float iCenter, float iMax, float fSpeed)
	{
		this(iServo);
		setLimits(iMin, iCenter, iMax);
	}
	
	public Robot_ServoMotor(int iServo, float iMin, float iCenter, float iMax, float fSpeed, boolean bInversed)
	{
		this(iServo);
		setLimits(iMin, iCenter, iMax);
		setInversed(bInversed);
	}	


	public void setLimits(float iMin, float iCenter, float iMax)
	{
		_fPositionMin = iMin;
		_fPositionCenter = iCenter;
		_fPositionMax = iMax;
	}
	
	private void setInversed(boolean bInversed) {
		_bInversed = bInversed;
	}
	
	public void setAxis(float fOffset, Vector3D vAxis) {
		_fAxisOffset = fOffset;
		_Axis = vAxis;
	}
	
	public void setSpeed(float fSpeed)
	{
		_fSpeedByDegree = fSpeed;
	}
	
	public void setBone(Bone bone)
	{
		_bone = bone;
	}

	public void setPosition(float fPos)
	{
		_fVirtualPosition = fPos;
		
		_fOldPosition = _fPosition;
		_fPosition = _fPositionCenter+((_bInversed)?-fPos:fPos);
		if(_fPosition>_fPositionMax) _fPosition = _fPositionMax;
		else if(_fPosition<_fPositionMin) _fPosition = _fPositionMin;
		
		if(_bone != null)
		{
			_bone.getQuaternion().setAxis(_fAxisOffset+_fPosition, _Axis);
		}
	}
	
	public void setRealPosition(float fRealPosition)
	{
		// TODO : Please Check that :)
		float fTemp = fRealPosition-_fPositionCenter;
		setPosition(_bInversed?fTemp:-fTemp);
	}

	public boolean haveChangedPosition()
	{
		return _fOldPosition!=getRealPosition();
	}
	
	public boolean isTransmittedHaveChangedPosition()
	{
		if(_fLastTransmitPosition == -1) return true;
		return _fLastTransmitPosition!=getRealPosition();
	}

	public float getCurrentPosition() {
		return _fVirtualPosition;
	}
	public float getRealPosition() {
		return _fPosition;
	}
	
	public void flagTransmitted() {
		_fLastTransmitPosition = getRealPosition();
	}
	public void flagUntransmitted() {
		_fLastTransmitPosition = -1;
	}

	public int getID() {
		return _iServo;
	}
}
